
/** ServoCommandSequence.cpp
 *
 *  Created on: Mar 14, 2010
 *      Author: As_You_Wish
 */

#include "ServoAction.h"
#include "Time.h"

#include "logging.h"

ServoAction::ServoAction(Servo* servo) {
	this->servo=servo;
	sequence = new LinkedList();
	next_update_time = 0;
	it = NULL;
	cyclic = true;
	forward = true;

}

ServoAction::~ServoAction() {
	delete (it);
	delete (sequence);
}

void ServoAction::reset(bool cyclic, bool forward) {
	this->forward = forward;
	delete (it);
	this->cyclic = cyclic;

	it = sequence->iterator(cyclic);
	next_update_time = 0;
}

void ServoAction::update() {
	if (Time::timer1_ms() > next_update_time) {

		ServoCommand* cmd;
		if (forward) {
			cmd = (ServoCommand*) it->next();
		} else {
			cmd = (ServoCommand*) it->previous();
		}

		if (cmd != NULL) {
			servo->state = cmd->state;
//			DEBUG("%s: %d\n", servo->name, servo->state.position);
			next_update_time = Time::timer1_ms() + cmd->duration;

		}
	}
}

void ServoAction::add(bool enabled, int position, uint32_t duration) {
	struct ServoCommand* cmd = new ServoCommand(enabled, position,duration);
	sequence->add(cmd);
}

bool ServoAction::isFinished() {
	return it->afterEnd || it->beforeStart;
}

